#include <rov/motor.h>
#include <rov/sensors.h>
#include <rov/misc.h>

#include <stdio.h>
#include <pthread.h>


#define CONTROLS_BUFFER_SIZE	8
#define SENSORS_BUFFER_SIZE	127
#define CONTROLS_PORT		32000
#define SENSORS_PORT		32003
#define SENSORS_CHAR		's'


int controlsCallback(volatile int * fd, volatile struct sockaddr_in * addr, socklen_t * len, Conn * client)
{
    char buf[CONTROLS_BUFFER_SIZE + 1];

    int amt = recvfrom(*fd, buf, CONTROLS_BUFFER_SIZE, 0, (struct sockaddr *) addr, len);

    if (amt == 0)
    {
        fprintf(stderr, "connection closed by client\n");
        close(*fd);
        *fd = -1;
        stopMotors();
        return -1;
    } else if (amt < 0)
    {
        fprintf(stderr, "error: connection closed\n");
        close(*fd);
        *fd = -1;
        stopMotors();
        return -1;
    }

fprintf(stderr, "recv: %d\n", amt);

    int devIndex = ntohl(* (int *) buf);
    int output = ntohl(* (int *) &buf[4]);

    setMotorSpeed(devIndex, output);

    return 0;
}


int sensorsCallback(Conn * client, char * buf, int len)
{
    if (client->fd >= 0)
    {
        int n = sendto(client->fd, buf, len, MSG_NOSIGNAL, (struct sockaddr *) &client->addr, sizeof(client->addr));

fprintf(stderr, ">");

        if (n < 0)
        {
            fprintf(stderr, "error: sensors connection closed\n");
            close(client->fd);
            client->fd = -1;
        }
    }

buf[len] = 0;
fprintf(stderr, "sensors: \"%s\"\n", buf);

    return 0;
}


void * sensorReaderEntry(void * conn)
{
    char buf[SENSORS_BUFFER_SIZE + 1];
    Conn * client = (Conn *) conn;

    int status = loopDataGather<Conn*>(sensorsCallback, client->sensor, buf, SENSORS_BUFFER_SIZE, '\n', client);

    if (status != 0)
        fprintf(stderr, "error reading sensor data\n");

    return NULL;
}


int main(int argc, char ** argv)
{
    Conn clients[2];

    clients[0].sensor = clients[1].sensor = initMotors();
    clients[0].fd     = clients[1].fd     = -1;

    clients[0].callback = controlsCallback;
    clients[1].callback = NULL;

    clients[0].port = CONTROLS_PORT;
    clients[1].port = SENSORS_PORT;

    printf("motor status: %d\n", clients[0].sensor);

    pthread_t sensorReader;
    pthread_t sensorServer;

    //pthread_create(&controlsServer, NULL, serverThreadEntry, &clients[0]);
    pthread_create(&sensorReader, NULL, sensorReaderEntry, &clients[1]);
    pthread_create(&sensorServer, NULL, serverThreadEntry, &clients[1]);

    serverThreadEntry(&clients[0]);

fprintf(stderr, "terminating\n");

    return 0;
}
